#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// 电机控制引脚定义
const int LEFT_MOTOR_PWM = 15;
const int LEFT_MOTOR_DIR = 13;
const int RIGHT_MOTOR_PWM = 14;
const int RIGHT_MOTOR_DIR = 2;

// OLED显示设置
#define SDA_PIN 16
#define SCL_PIN 17
#define SCREEN_WIDTH 128    // OLED显示宽度，单位:像素
#define SCREEN_HEIGHT 64    // OLED显示高度，单位:像素
#define OLED_RESET    -1    // Reset pin # (或-1表示不使用重置引脚)
#define SCREEN_ADDRESS 0x3C ///< OLED I2C地址，通常为0x3C或0x3D

// 输入设备引脚
const int POT_PIN = 33;       // 电位器连接到GPIO33
const int THROTTLE_SWITCH_PIN = 25;  // 脚踏开关连接到GPIO25
const int JOYSTICK_PIN = 32;  // 摇杆按钮连接到GPIO32

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  Serial.begin(115200);  // 初始化串口通信
  
  // 初始化电机控制引脚
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  
  // 初始化自定义I2C引脚
  Wire.begin(SDA_PIN, SCL_PIN);
  
  // 初始化OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 display initialization failed"));
    for(;;); // 停止执行，进入无限循环
  }
  
  // 配置ADC设置
  analogSetAttenuation(ADC_11db);  // 设置11dB衰减，允许测量0-3.3V范围
  analogReadResolution(12);        // 设置12位分辨率(0-4095)
  
  // 配置脚踏开关引脚：输入模式，启用内部上拉电阻
  pinMode(THROTTLE_SWITCH_PIN, INPUT_PULLUP);
  
  // 配置摇杆按钮引脚：输入模式，启用内部上拉电阻
  pinMode(JOYSTICK_PIN, INPUT_PULLUP);
  
  // 清屏并显示初始信息
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Motor Control System");
  display.println("Initializing...");
  display.display();
  delay(1000);
}

void loop() {
  // 读取脚踏开关状态：LOW表示踩下，HIGH表示未踩下
  bool isThrottlePressed = digitalRead(THROTTLE_SWITCH_PIN) == HIGH;
  
  // 读取摇杆按钮状态：LOW表示按下，HIGH表示未按下
  bool isJoystickPressed = digitalRead(JOYSTICK_PIN) == LOW;
  
  int pwmValue = 0; // 确保在所有情况下都定义了pwmValue

  // 只有在踩下脚踏开关时，才读取电位器值并控制电机速度
  if (isThrottlePressed) {
    // 读取电位器原始值(0-4095)
    int sensorValue = analogRead(POT_PIN);
    
    // 将原始值转换为PWM值(0-255)
    pwmValue = map(sensorValue, 0, 4095, 0, 255);
    
    // 根据摇杆按钮状态设置电机方向
    if (isJoystickPressed) {
      // 摇杆按下，电机反转
      digitalWrite(LEFT_MOTOR_DIR, LOW);  
      digitalWrite(RIGHT_MOTOR_DIR, LOW); 
    } else {
      // 摇杆未按下，电机正转
      digitalWrite(LEFT_MOTOR_DIR, HIGH);  
      digitalWrite(RIGHT_MOTOR_DIR, HIGH); 
    }
    
    // 设置电机速度
    analogWrite(LEFT_MOTOR_PWM, pwmValue);
    analogWrite(RIGHT_MOTOR_PWM, pwmValue);
  } else {
    // 未踩下脚踏开关，电机停止
    analogWrite(LEFT_MOTOR_PWM, 0);
    analogWrite(RIGHT_MOTOR_PWM, 0);
  }
  
  // 输出到串口监视器
  Serial.print("Throttle: ");
  Serial.print(isThrottlePressed ? "ON" : "OFF");
  Serial.print(" | Joystick: ");
  Serial.print(isJoystickPressed ? "PRESSED" : "RELEASED");
  Serial.print(" | Speed: ");
  Serial.println(pwmValue);
  
  // 更新OLED显示
  display.clearDisplay();
  
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Throttle: ");
  display.print(isThrottlePressed ? "ON" : "OFF");
  
  display.setCursor(0, 10);
  display.print("Joystick: ");
  display.print(isJoystickPressed ? "PRESSED" : "RELEASED");
  
  display.setCursor(0, 20);
  display.print("Speed: ");
  display.print(pwmValue);
  
  display.display();  // 更新显示
  
  delay(100);  // 延时100毫秒，控制更新频率
}